#include "esp_system.h"
#include "string.h"
#include "driver/i2c.h"

#include "PCA9535Driver.h"


#define I2C_MASTER_PORT_NUM        I2C_NUM_0

#define PAC9535_REG_INPUT_0         0x00
#define PAC9535_REG_INPUT_1         0x01
#define PAC9535_REG_OUTPUT_0        0x02
#define PAC9535_REG_OUTPUT_1        0x03
#define PAC9535_REG_POLARITY_0      0x04
#define PAC9535_REG_POLARITY_1      0x05
#define PAC9535_REG_CONFIG_0        0x06
#define PAC9535_REG_CONFIG_1        0x07

static esp_err_t pac9535_read_byte(uint8_t reg_addr, uint8_t *data) ;
static esp_err_t pac9535_write_byte(uint8_t reg_addr, uint8_t data) ;

static esp_err_t pac9535_write_byte(uint8_t reg_addr, uint8_t data) 
{
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (PAC9535_ADDR << 1) | I2C_MASTER_WRITE, true);
    // i2c_master_write_byte(cmd, PAC9535_ADDR, true);
    i2c_master_write_byte(cmd, reg_addr, true);
    i2c_master_write_byte(cmd, data, true);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin(I2C_MASTER_PORT_NUM, cmd, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd);
    return ret;
}

static esp_err_t pac9535_read_byte(uint8_t reg_addr, uint8_t *data) 
{
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (PAC9535_ADDR << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write_byte(cmd, reg_addr, true);
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (PAC9535_ADDR << 1) | I2C_MASTER_READ, true);
    i2c_master_read_byte(cmd, data, I2C_MASTER_NACK);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin(I2C_MASTER_PORT_NUM, cmd, 1000 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd);
    return ret;
}

void pac9535_init(uint8_t port1fun, uint8_t port2fun, uint8_t outlvl) 
{
    // 将所有端口设置为输出模式(0=输出, 1=输入)
    pac9535_write_byte(PAC9535_REG_CONFIG_0, port1fun);
    pac9535_write_byte(PAC9535_REG_CONFIG_1, port2fun);
    
    // 初始化输出为低电平
    if (port1fun == PCA_OUTPUT)
    {
        pac9535_write_byte(PAC9535_REG_OUTPUT_0, outlvl);
    }
    if (port2fun == PCA_OUTPUT)
    {
        pac9535_write_byte(PAC9535_REG_OUTPUT_1, outlvl);
    }
    // 极性不反转(可选)
    pac9535_write_byte(PAC9535_REG_POLARITY_0, 0x00);
    pac9535_write_byte(PAC9535_REG_POLARITY_1, 0x00);
}

void pac9535_set_output(uint8_t port, PCA_Pin_t pin, bool state) 
{
    uint8_t reg = (port == 0) ? PAC9535_REG_OUTPUT_0 : PAC9535_REG_OUTPUT_1;
    uint8_t current_state;
    if (pin == pinAll)
    {
        if(state)
        {
            current_state = 0xff;
        }
        else
        {
            current_state = 0;
        }
    }
    else
    {
        pac9535_read_byte(reg, &current_state);
        if (state) 
        {
            current_state |= (1 << pin);
        } else 
        {
            current_state &= ~(1 << pin);
        }
    }
    pac9535_write_byte(reg, current_state);
}

uint8_t pac9535_get_input(uint8_t port, PCA_Pin_t pin) 
{
    uint8_t reg = (port == 0) ? PAC9535_REG_INPUT_0 : PAC9535_REG_INPUT_1;
    uint8_t input_state;
    
    pac9535_read_byte(reg, &input_state);
    if (pin == pinAll)
    {
        return input_state;
    }
    else
    {
        return (input_state & (1 << pin)) ? 1 : 0;
    }
}